import pandas as pd
import matplotlib.pyplot as plt

# 读取 CSV 文件，第一行是表头
df = pd.read_csv('~/UAV/src/ag_robot/scripts/ghr_brain_auto_C-2.csv')  # 替换为你的文件名

# # 提取各列数据
# # 第1列是 Leader X，第2列是 Leader Y
# leader_x = df.iloc[:, 0].to_numpy()   # 转为 NumPy 数组
# leader_y = df.iloc[:, 1].to_numpy()   # 转为 NumPy 数组

# follower1_x = df.iloc[:, 4].to_numpy()
# follower1_y = df.iloc[:, 5].to_numpy()

# # 如果你画了 follower2，也一样处理：
# follower2_x = df.iloc[:, 8].to_numpy()
# follower2_y = df.iloc[:, 9].to_numpy()

# 提取各列数据，忽略第一行（即从第二行开始，索引为1）
leader_x = df.iloc[1:, 0].to_numpy()   # 第1列（Leader X），从第2行开始
leader_y = df.iloc[1:, 1].to_numpy()   # 第2列（Leader Y），从第2行开始

follower1_x = df.iloc[1:, 4].to_numpy()  # 第5列（Follower1 X）
follower1_y = df.iloc[1:, 5].to_numpy()  # 第6列（Follower1 Y）

follower2_x = df.iloc[1:, 8].to_numpy()  # 第9列（Follower2 X）
follower2_y = df.iloc[1:, 9].to_numpy()  # 第10列（Follower2 Y）
# 创建画布
plt.figure(figsize=(10, 6))

# 绘制 Leader 轨迹 —— 蓝色实线
plt.plot(leader_x, leader_y, 'b-', label='Leader (X,Y)', linewidth=2)

# 绘制 Follower1 轨迹 —— 绿色实线
plt.plot(follower1_x, follower1_y, 'g-', label='Follower1 (X,Y)', linewidth=2)

# （可选）绘制 Follower2 轨迹 —— 红色实线
plt.plot(follower2_x, follower2_y, 'r-', label='Follower2 (X,Y)', linewidth=2)

# 图形美化
plt.xlabel('X 坐标')
plt.ylabel('Y 坐标')
plt.title('多智能体轨迹对比图（Leader & Followers）')
plt.legend()
plt.grid(True)
plt.axis('equal')  # 保持 x y 比例一致，防止图形拉伸

# 显示图形
plt.show()